#include "socketlib.h"

#include<iostream>
#include <time.h>
using namespace std;
void recv_data()
{
    const unsigned short SERVERPORT = 6100;
    const int BACKLOG = 10; //10 个最大的连接数
    const int MAXSIZE = 1024;
    int sock, client_fd;
    sockaddr_in myAddr;
    sockaddr_in remoteAddr;
    sock = socket(AF_INET, SOCK_STREAM, 0);
    //create socket
    if( sock == -1)
    {
        cerr<<"socket create fail!"<<endl;
        exit(1);
    }

    cout<<"sock :"<<sock<<endl;

    //bind
    myAddr.sin_family = AF_INET;
    myAddr.sin_port = htons(SERVERPORT);
    myAddr.sin_addr.s_addr = INADDR_ANY;
    bzero( &(myAddr.sin_zero), 8);
    if(bind(sock, (sockaddr*)(&myAddr), sizeof(sockaddr)) ==-1 )
    {
        cerr<<"bind error!"<<endl;
        exit(1);
    }

    //listen
    if(listen(sock, BACKLOG) == -1)
    {
        cerr<<"listen error"<<endl;
        exit(1);
    }

    robot_info rev_packet;
    unsigned int sin_size = sizeof(sockaddr_in);
    while( 1 )
    {
        if((client_fd = accept(sock, (sockaddr*)(&remoteAddr), &sin_size)) ==-1)continue;
        cout<<"Received a connection from "<<static_cast<char*>(inet_ntoa(remoteAddr.sin_addr) )<<endl;
        while(1)
        {
            int iret;
            char buf[1024];

            iret = recv(client_fd, buf, 112, 0);

            time_t nowtime;
            nowtime = time(NULL); //获取当前时间
            cout << ctime(&nowtime) ;
            //std::cout<<"time::" << nowtime << endl;
            if(iret<=0){ break;}
            if(iret==112) {
                buf[iret] = '\0';
                memset(&rev_packet, 0, sizeof(rev_packet));
                memcpy(&rev_packet, buf, sizeof(rev_packet));
                cout<<"cmd:["<<rev_packet.cmd1<<","<<rev_packet.cmd2<<","<<rev_packet.cmd3<<","<<rev_packet.cmd4<<"]"<<endl;
                cout << "pose_l:[" << rev_packet.l_x << "," << rev_packet.l_y << "," << rev_packet.l_z << ","
                     << rev_packet.l_a << ","
                     << rev_packet.l_b << "," << rev_packet.l_c << "]" << endl;
                cout << "angle_l:[" << rev_packet.l_angel1 << "," << rev_packet.l_angel2 << "," << rev_packet.l_angel3
                     << "," << rev_packet.l_angel4 << ","
                     << rev_packet.l_angel5 << "," << rev_packet.l_angel6 << "]" << endl;

                cout << "pose_r:[" << rev_packet.r_x << "," << rev_packet.r_y << "," << rev_packet.r_z << ","
                     << rev_packet.r_a << ","
                     << rev_packet.r_b << "," << rev_packet.r_c << "]" << endl;
                cout << "angle_r:[" << rev_packet.r_angel1 << "," << rev_packet.r_angel2 << "," << rev_packet.r_angel3
                     << "," << rev_packet.r_angel4 << ","
                     << rev_packet.r_angel5 << "," << rev_packet.r_angel6 << "]" << endl;
                cout<<"wheel:["<<rev_packet.left_wheel<<","<<rev_packet.right_wheel<<","<<rev_packet.up_down<<"]"<<endl;
                cout<<endl;
            }

            sleep(0.1);
        }
        cout<<"close"<<endl;
        close(client_fd);
    }

}


int main()
{
    // send message 
    recv_data();
}



